#include <Bounce.h>

const int sensRR = A2;
const int sensMR = A3;
const int sensML = A4;
const int sensLL = 6;
const int encR = 3; const int encRLED = 11;
const int encL = 2; const int encLLED = 12;

const int ddelay = 100;

Bounce RR = Bounce(sensRR, ddelay); 
Bounce MR = Bounce(sensMR, ddelay); 
Bounce ML = Bounce(sensML, ddelay); 
Bounce LL = Bounce(sensLL, ddelay); 

int count = 0;
int lastSens;

//radEncoder
volatile int iencL = 0;
volatile int iencR = 0;

void sensSetup(){
  pinMode(sensLL, INPUT);
  pinMode(encR, INPUT);
  pinMode(encL, INPUT);
  pinMode(encRLED, OUTPUT);
  pinMode(encLLED, OUTPUT);
  attachInterrupt(1, encoderR, CHANGE);
  attachInterrupt(0, encoderL, CHANGE);
}

void testEnc(){
  set(encRLED, digitalRead(encR));
  set(encLLED, digitalRead(encL));
}

int sens(){  //1= right; 2= MR; 3 =ML; 4=Left
  int val = 0;
  int res = 0;
  
  RR.update();
  MR.update();
  ML.update();
  LL.update();
  
  val = RR.read();
  if(val == LOW) {res = 1;}
  val = MR.read();
  if(val == LOW) {res = 2;}
  val = ML.read();
  if(val == LOW) {res = 3;}
  val = LL.read();
  if(val == LOW) {res = 4;} 
  
  /*
  count++;
  if((millis()-startTime)>1000){
    Serial.print("Sens: ");
    Serial.print(count);
    Serial.println("mal pro Sek");
    delay(2);
    count=0;
    startTime=millis(); 
  } */
  
  if(lastSens != res){
    Serial.print("Sens: ");  
    Serial.println(res);
    lastSens = res;
    delay(1);
  }
  return res;
}

void encoderL(){
  iencL++;
  Serial.print("encL: ");  
  Serial.println(iencL);
}

void encoderR(){
  iencR++;
  Serial.print("encR: ");  
  Serial.println(iencR);
} 

